/*
 * Alarms.cpp
 *
 *  Created on: Jan 21, 2014
 *      Author: ppks
 */

#include "common.h"
#include "FlightController_Common.h"
#include "Alarms.h"
#include "i2c.h"

/******************************************************************************/
/* Global Variables Declaration                                               */
/******************************************************************************/
uint8_t Alarms_AlarmArray[ALARM_EVENTS_NUM];

/******************************************************************************/
/* Local Variables Declaration                                                */
/******************************************************************************/
static uint8_t Alarm_CycleDone[ALARM_RESOURCES_NUM];
static uint8_t Alarm_ResourceIsOn[ALARM_RESOURCES_NUM];
static uint8_t Alarm_SequenceActive[ALARM_RESOURCES_NUM];
static uint32_t Alarm_LastToggleTime[ALARM_RESOURCES_NUM];
static int16_t  i2c_errors_count_old = 0;

/******************************************************************************/
/* Local Functions Declaration                                                */
/******************************************************************************/
static void Alarms_SetTiming(uint8_t resource, uint16_t pulse, uint16_t pause);
static void Alarms_ToggleResource(uint8_t resource, uint8_t activate);
static void Alarms_TurnOffResource(uint8_t resource);
static void Alarms_PatternCompose(void);
static void Alarms_PatternDecode(uint8_t resource, uint16_t first, uint16_t second,uint16_t third, uint16_t cyclepause, uint16_t endpause);
#if defined(PILOTLAMP)
static void PilotLampSequence(uint16_t speed, uint16_t pattern, uint8_t num_patterns);
static void PilotLamp(uint8_t count);
#endif /* defined(PILOTLAMP) */

/******************************************************************************/
/* Function Code                                                              */
/******************************************************************************/
/*******************************************************************************
* Function Name: Alarms_Init
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void Alarms_Init(void)
{
    /* Initialize the arrays with 0 values */
    memset(&Alarm_CycleDone, 0, sizeof(Alarm_CycleDone));
    memset(&Alarm_ResourceIsOn, 0, sizeof(Alarm_ResourceIsOn));
    memset(&Alarm_SequenceActive, 0, sizeof(Alarm_SequenceActive));
    memset(&Alarm_LastToggleTime, 0, sizeof(Alarm_LastToggleTime));

#if defined (PILOTLAMP)
    /* Init Pilot Lamp */
    PL_INIT;
#endif /* defined (PILOTLAMP) */
}

/*******************************************************************************
* Function Name: Alarms_ProcessHandler
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void Alarms_ProcessHandler(void)
{
#if defined(RCOPTIONSBEEP)
    static uint8_t checkboxItemIndex = 0;
    static uint8_t firstrun = 1;
    static uint8_t last_rcOptions[CHECKBOXITEMS];

    if (last_rcOptions[checkboxItemIndex] != rcOptions[checkboxItemIndex])
    {
        Alarms_AlarmArray[ALARM_EVENT_TOGGLE] = ALARM_LEVEL_1;
    }

    last_rcOptions[checkboxItemIndex] = rcOptions[checkboxItemIndex];
    checkboxItemIndex++;

    if (checkboxItemIndex >= CHECKBOXITEMS)
    {
        checkboxItemIndex = 0;
    }

    if ((firstrun == 1) && (Alarms_AlarmArray[ALARM_EVENT_CONFIRMATION] == ALARM_LEVEL_NONE))
    {
        Alarms_AlarmArray[ALARM_EVENT_TOGGLE] = ALARM_LEVEL_NONE;    /* only enable options beep AFTER gyro init */
        Alarms_AlarmArray[ALARM_EVENT_BEEPER_ON] = ALARM_LEVEL_NONE;
    }
    else
    {
        firstrun = 0;
    }
#endif /* defined(RCOPTIONSBEEP) */

#if defined(FAILSAFE)
    if (Failsafe_IsRCFailsafeActive() && globalFlags.ARMED)
    {
        Alarms_AlarmArray[ALARM_EVENT_FAILSAFE] = ALARM_LEVEL_1;            /* set failsafe warning level to 1 while landing */
        if (Failsafe_IsTimeToStopMotors())
        {
            Alarms_AlarmArray[ALARM_EVENT_FAILSAFE] = ALARM_LEVEL_2;        /* start "find me" signal after landing */
        }
    }
    else if (Failsafe_IsRCFailsafeActive() && !globalFlags.ARMED)
    {
        Alarms_AlarmArray[ALARM_EVENT_FAILSAFE] = ALARM_LEVEL_2;            /* tx turned off while motors are off: start "find me" signal */
    }
    if (!Failsafe_IsRCFailsafeActive())
    {
        Alarms_AlarmArray[ALARM_EVENT_FAILSAFE] = ALARM_LEVEL_NONE;         /* turn off alarm if TX is okay */
    }
#endif /* defined(FAILSAFE) */

#if GPS_INSTALLED
    if ((globalFlags.GPS_HOME_MODE || globalFlags.GPS_HOLD_MODE) && !globalFlags.GPS_FIX)
    {
        Alarms_AlarmArray[ALARM_EVENT_NO_GPS] = ALARM_LEVEL_2;
    }
    else if (!globalFlags.GPS_FIX)
    {
        Alarms_AlarmArray[ALARM_EVENT_NO_GPS] = ALARM_LEVEL_1;
    }
    else Alarms_AlarmArray[ALARM_EVENT_NO_GPS] = ALARM_LEVEL_NONE;
#endif /* GPS_INSTALLED */

#if defined(BUZZER)
    if (rcOptions[BOXBEEPERON])
    {
        Alarms_AlarmArray[ALARM_EVENT_BEEPER_ON] = ALARM_LEVEL_1;
    }
    else
    {
        Alarms_AlarmArray[ALARM_EVENT_BEEPER_ON] = ALARM_LEVEL_NONE;
    }
#endif /* defined(BUZZER) */

#if defined(POWERMETER)
    if ((pMeter[PMOTOR_SUM] < pAlarm) || (pAlarm == 0) || !globalFlags.ARMED)
    {
        Alarms_AlarmArray[ALARM_EVENT_POWER_METER] = ALARM_LEVEL_NONE;
    }
    else if (pMeter[PMOTOR_SUM] > pAlarm)
    {
        Alarms_AlarmArray[ALARM_EVENT_POWER_METER] = ALARM_LEVEL_1;
    }
#endif /* defined(POWERMETER) */

#if defined(ARMEDTIMEWARNING)
    if ((ArmedTimeWarningMicroSeconds > 0) && (armedTime >= ArmedTimeWarningMicroSeconds) && globalFlags.ARMED)
    {
        Alarms_AlarmArray[ALARM_EVENT_RUNTIME] = ALARM_LEVEL_1;
    }
    else
    {
        Alarms_AlarmArray[ALARM_EVENT_RUNTIME] = ALARM_LEVEL_NONE;
    }
#endif /* defined(ARMEDTIMEWARNING) */

#if defined(VBAT_ENABLED)
    if (vbatMin < FC_Config.MISC.vbatlevel_crit)
    {
        Alarms_AlarmArray[ALARM_EVENT_V_BAT] = ALARM_LEVEL_4;
    }
    else if ((analogData.vbat > FC_Config.MISC.vbatlevel_warn1)  || (NO_VBAT > analogData.vbat))
    {
        Alarms_AlarmArray[ALARM_EVENT_V_BAT] = ALARM_LEVEL_NONE;
    }
    else if (analogData.vbat > FC_Config.MISC.vbatlevel_warn2)
    {
        Alarms_AlarmArray[ALARM_EVENT_V_BAT] = ALARM_LEVEL_1;
    }
    else if (analogData.vbat > FC_Config.MISC.vbatlevel_crit)
    {
        Alarms_AlarmArray[ALARM_EVENT_V_BAT] = ALARM_LEVEL_2;
    }
#endif /* defined(VBAT_ENABLED) */

    if ((i2c_errors_count > i2c_errors_count_old + 100) || (i2c_errors_count < -1))
    {
        Alarms_AlarmArray[ALARM_EVENT_I2C_ERROR] = ALARM_LEVEL_1;
    }
    else
    {
        Alarms_AlarmArray[ALARM_EVENT_I2C_ERROR] = ALARM_LEVEL_NONE;
    }

    /*  Compose the alarm pattern based on Alarms_AlarmArray flags */
    Alarms_PatternCompose();
}

/*******************************************************************************
* Function Name: Alarms_SetTiming
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void Alarms_SetTiming(uint8_t resource, uint16_t pulse, uint16_t pause)
{
    if (!Alarm_ResourceIsOn[resource] && (millis() >= (Alarm_LastToggleTime[resource] + pause)) && (pulse != 0))
    {
        Alarm_ResourceIsOn[resource] = 1;
        Alarms_ToggleResource(resource,1);
        Alarm_LastToggleTime[resource]=millis();
    }
    else if (Alarm_ResourceIsOn[resource] && ((millis() >= Alarm_LastToggleTime[resource] + pulse)  || (pulse == 0)))
    {
        Alarm_ResourceIsOn[resource] = 0;
        Alarms_ToggleResource(resource,0);
        Alarm_LastToggleTime[resource]=millis();
        Alarm_CycleDone[resource] = 1;
    }
}

/*******************************************************************************
* Function Name: Alarms_ToggleResource
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void Alarms_ToggleResource(uint8_t resource, uint8_t activate)
{
    switch(resource)
    {
    #if defined(BUZZER)
        case RESOURCE_BUZZER:
            if (activate == 1) {BUZZERPIN_ON;}
            else {BUZZERPIN_OFF;}
            break;
    #endif /* defined(BUZZER) */

    #if defined (PILOTLAMP)
        case RESOURCE_PL_GREEN:
            if (activate == 1) {PilotLamp(PL_GRN_ON);}
            else {PilotLamp(PL_GRN_OFF);}
            break;

        case RESOURCE_PL_BLUE:
            if (activate == 1) {PilotLamp(PL_BLU_ON);}
            else {PilotLamp(PL_BLU_OFF);}
            break;

        case RESOURCE_PL_RED:
            if (activate == 1) {PilotLamp(PL_RED_ON);}
            else {PilotLamp(PL_RED_OFF);}
            break;
    #endif /* defined (PILOTLAMP) */

        case RESOURCE_LED:
        default:
            if (activate == 1) {LEDPIN_ON;}
            else {LEDPIN_OFF;}
            break;
    }
    return;
}

/*******************************************************************************
* Function Name: Alarms_TurnOffResource
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void Alarms_TurnOffResource(uint8_t resource)
{
    switch(resource)
    {
        case RESOURCE_LED:
            if (Alarm_ResourceIsOn[resource])
            {
                Alarm_ResourceIsOn[resource] = 0;
                LEDPIN_OFF;
            }
            break;

        case RESOURCE_BUZZER:
            if (Alarm_ResourceIsOn[resource])
            {
                Alarm_ResourceIsOn[resource] = 0;
                BUZZERPIN_OFF;
            }
            break;

        case RESOURCE_PL_GREEN:
            if (Alarm_ResourceIsOn[resource])
            {
                Alarm_ResourceIsOn[resource] = 0;
            #if defined(PILOTLAMP)
                PilotLamp(PL_GRN_OFF);
            #endif /* defined(PILOTLAMP) */
            }
            break;

        case RESOURCE_PL_BLUE:
            if (Alarm_ResourceIsOn[resource])
            {
                Alarm_ResourceIsOn[resource] = 0;
            #if defined(PILOTLAMP)
                PilotLamp(PL_BLU_OFF);
            #endif /* defined(PILOTLAMP) */
            }
            break;

        case RESOURCE_PL_RED:
            if (Alarm_ResourceIsOn[resource])
            {
                Alarm_ResourceIsOn[resource] = 0;
            #if defined(PILOTLAMP)
                PilotLamp(PL_RED_OFF);
            #endif /* defined(PILOTLAMP) */
            }
            break;
    }
}

/*******************************************************************************
* Function Name: Alarms_PatternCompose
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void Alarms_PatternCompose(void)
{
    static char resource = RESOURCE_LED;

    /* patternDecode(length1,length2,length3,beeppause,endpause,loop) */
#if defined(BUZZER)
    resource = RESOURCE_BUZZER;                                                              /* buzzer selected */

    if (Alarms_AlarmArray[ALARM_EVENT_FAILSAFE] == ALARM_LEVEL_2)
    {
        Alarms_PatternDecode(resource,200,0,0,50,2000);                         /* failsafe "find me" signal */
    }
    else if ((Alarms_AlarmArray[ALARM_EVENT_FAILSAFE] == ALARM_LEVEL_1) || (Alarms_AlarmArray[ALARM_EVENT_ACCELEROMETER] == ALARM_LEVEL_1))
    {
        Alarms_PatternDecode(resource,50,200,200,50,50);                        //failsafe "panic"  or Acc not calibrated
    }
    else if (Alarms_AlarmArray[ALARM_EVENT_TOGGLE] == ALARM_LEVEL_1)
    {
        Alarms_PatternDecode(resource,50,0,0,50,0);                             /* toggle 1 */
    }
    else if (Alarms_AlarmArray[ALARM_EVENT_TOGGLE] == ALARM_LEVEL_2)
    {
        Alarms_PatternDecode(resource,50,50,0,50,0);                            /* toggle 2 */
    }
    else if (Alarms_AlarmArray[ALARM_EVENT_TOGGLE] > ALARM_LEVEL_2)
    {
        Alarms_PatternDecode(resource,50,50,50,50,0);                           /* toggle other */
    }
    else if (Alarms_AlarmArray[ALARM_EVENT_NO_GPS] == ALARM_LEVEL_2)
    {
        Alarms_PatternDecode(resource,50,50,0,50,50);                           /* gps installed but no fix */
    }
    else if (Alarms_AlarmArray[ALARM_EVENT_BEEPER_ON] == ALARM_LEVEL_1)
    {
        Alarms_PatternDecode(resource,50,50,50,50,50);                          /* BeeperOn */
    }
    else if (Alarms_AlarmArray[ALARM_EVENT_POWER_METER] == ALARM_LEVEL_1)
    {
        Alarms_PatternDecode(resource,50,50,0,50,120);                          /* pMeter Warning */
    }
    else if (Alarms_AlarmArray[ALARM_EVENT_RUNTIME] == ALARM_LEVEL_1)
    {
        Alarms_PatternDecode(resource,50,50,50,50,0);                           /* Runtime warning */
    }
    else if (Alarms_AlarmArray[ALARM_EVENT_V_BAT] == 4)
    {
        Alarms_PatternDecode(resource,50,50,200,50,2000);                       /* vbat critical */
    }
    else if (Alarms_AlarmArray[ALARM_EVENT_V_BAT] == ALARM_LEVEL_2)
    {
        Alarms_PatternDecode(resource,50,200,0,50,2000);                        /* vbat warning */
    }
    else if (Alarms_AlarmArray[ALARM_EVENT_V_BAT] == ALARM_LEVEL_1)
    {
        Alarms_PatternDecode(resource,200,0,0,50,2000);                         /* vbat info */
    }
    else if (Alarms_AlarmArray[ALARM_EVENT_CONFIRMATION] == ALARM_LEVEL_1)
    {
        Alarms_PatternDecode(resource,200,0,0,50,200);                          /* confirmation indicator 1x */
    }
    else if (Alarms_AlarmArray[ALARM_EVENT_CONFIRMATION] == ALARM_LEVEL_2)
    {
        Alarms_PatternDecode(resource,200,200,0,50,200);                        /* confirmation indicator 2x */
    }
    else if (Alarms_AlarmArray[ALARM_EVENT_CONFIRMATION] > ALARM_LEVEL_2)
    {
        Alarms_PatternDecode(resource,200,200,200,50,200);                      /* confirmation indicator 3x */
    }
    else if (Alarm_SequenceActive[(uint8_t)resource] == ALARM_LEVEL_1)
    {
        Alarms_PatternDecode(resource,0,0,0,0,0);                               /* finish last sequence if not finished yet */
    }
    else
    {
        Alarms_TurnOffResource(resource);                                       /* turn off the resource */
    }

    Alarms_AlarmArray[ALARM_EVENT_ACCELEROMETER] = ALARM_LEVEL_NONE;            /* reset acc not calibrated */

#endif /* defined(BUZZER) */

#if defined(PILOTLAMP)
    if (Alarms_AlarmArray[ALARM_EVENT_I2C_ERROR] == ALARM_LEVEL_1)
    {
        PilotLampSequence(100,B000111,2);                                       /* I2C Error */
    }
    else if (Alarms_AlarmArray[ALARM_EVENT_BEEPER_ON] == ALARM_LEVEL_1)
    {
        PilotLampSequence(100,B0101<<8|B00010001,4);                            /* BeeperOn */
    }
    else
    {
        resource = RESOURCE_PL_GREEN;
        if (globalFlags.ARMED && globalFlags.ANGLE_MODE)
        {
            Alarms_PatternDecode(resource,100,100,100,100,1000);                /* Green Slow Blink-->angle */
        }
        else if (globalFlags.ARMED && globalFlags.HORIZON_MODE)
        {
            Alarms_PatternDecode(resource,200,200,200,100,1000);                /* Green mid Blink-->horizon */
        }
        else if (globalFlags.ARMED)
        {
            Alarms_PatternDecode(resource,100,100,0,100,1000);                  /* Green fast Blink-->acro */
        }
        else
        {
            Alarms_TurnOffResource(resource);                                   /* switch off */
        }

        resource = RESOURCE_PL_BLUE;
    #if GPS_INSTALLED
        if (Alarms_AlarmArray[ALARM_EVENT_NO_GPS] == ALARM_LEVEL_1)
        {
            Alarms_PatternDecode(resource,100,100,100,100,100);                 /* blue fast blink -->no gps fix */
        }
        else if (globalFlags.GPS_HOME_MODE || globalFlags.GPS_HOLD_MODE)
        {
            Alarms_PatternDecode(resource,100,100,100,100,1000);                /* blue slow blink --> gps active */
        }
        else
        {
            Alarms_SetTiming(resource,100,1000);                                /*blue short blink -->gps fix ok */
        }
    #else /* GPS_INSTALLED */
        Alarms_TurnOffResource(resource);
    #endif /* GPS_INSTALLED */

        resource = RESOURCE_PL_RED;
        if (Alarms_AlarmArray[ALARM_EVENT_FAILSAFE] == ALARM_LEVEL_1)
        {
            Alarms_SetTiming(resource,100,100);                                 /* Red fast blink--> failsafe panic */
        }
        else if (Alarms_AlarmArray[ALARM_EVENT_FAILSAFE] == ALARM_LEVEL_2)
        {
            Alarms_PatternDecode(resource,1000,0,0,0,2000);                     /* red slow blink--> failsafe find me */
        }
        else
        {
            Alarms_TurnOffResource(resource);
        }
    }
#endif /* defined(PILOTLAMP) */
}

/*******************************************************************************
* Function Name: Alarms_PatternDecode
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void Alarms_PatternDecode(uint8_t resource, uint16_t first, uint16_t second, uint16_t third, uint16_t cyclepause, uint16_t endpause)
{
    static uint16_t pattern[ALARM_RESOURCES_NUM][5];
    static uint8_t icnt[ALARM_RESOURCES_NUM] = {0,0,0,0,0};

    if (Alarm_SequenceActive[resource] == 0)
    {
        Alarm_SequenceActive[resource] = 1;
        pattern[resource][0] = first;
        pattern[resource][1] = second;
        pattern[resource][2] = third;
        pattern[resource][3] = endpause;
        pattern[resource][4] = cyclepause;
    }

    if (icnt[resource] <3)
    {
        if (pattern[resource][icnt[resource]] != 0)
        {
            Alarms_SetTiming(resource,pattern[resource][icnt[resource]],pattern[resource][4]);
        }
    }
    else if (Alarm_LastToggleTime[resource] < (millis()-pattern[resource][3]))
    {
        /* sequence is over: reset everything */
        icnt[resource]=0;
        Alarm_SequenceActive[resource] = 0;       /* sequence is now done, cycleDone sequence may begin */
        Alarms_AlarmArray[0] = 0;                 /* reset toggle bit */
        Alarms_AlarmArray[7] = 0;                 /* reset confirmation bit */
        Alarms_TurnOffResource(resource);
        return;
    }

    if ((Alarm_CycleDone[resource] == 1) || (pattern[resource][icnt[resource]] == 0))
    {
        /* single on off cycle is done */

        if (icnt[resource] < 3)
        {
            icnt[resource]++;
        }

        Alarm_CycleDone[resource] = 0;
        Alarms_TurnOffResource(resource);
    }
}

/*******************************************************************************
* Function Name: Alarms_BlinkLED
********************************************************************************
* Summary:
*  Blinks the LEDPIN num times with the period ontime*2 and repeats it "repet"
*  number of times.
*  !!! Beware - this function is blocking and should not be used in flight.
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void Alarms_BlinkLED(uint8_t num, uint8_t ontime,uint8_t repeat)
{
    uint8_t i;
    uint8_t r;

    for (r = 0; r < repeat; r++)
    {
        for(i = 0; i < num; i++)
        {
            /* switch LEDPIN state */
            LEDPIN_TOGGLE;
            delay(ontime);
        }

        /* wait 60 ms */
        delay(60);
    }
}

#if defined(PILOTLAMP)
/********************************************************************/
/*                      Pilot Lamp Handling                         */
/********************************************************************/
/* original code based on mr.rc-cam and jevermeister work */
/* modified by doughboy to use timer interrupt */
/* define your light pattern by bits, 0=off 1=on
 * define up to 5 patterns that cycle using 15 bits, pattern starts at bit 0 in groups of 3
 *  14 13 12 11 10 9 8 7 6 5 4 3 2 1 0
 *   R  B  G  R  B G R B G R B G R B G
 * parameters speed is the ms between patterns
 *           pattern is the 16bit (uses only 15 bits) specifying up to 5 patterns
 *           num_patterns is the number of patterns defined
 * example to define sequential light where G->B->R then back to G, you need 4 patterns
 *           B00000101<<8 | B00010001
 * example: to define alternate all on and al off, you only need two patterns
 *           B00000111
 */

#define PL_BUF_SIZE     8

volatile uint8_t queue[PL_BUF_SIZE]; /* circular queue */
volatile uint8_t head = 0;
volatile uint8_t tail = 0;

/*******************************************************************************
* Function Name: PilotLampSequence
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void PilotLampSequence(uint16_t speed, uint16_t pattern, uint8_t num_patterns)
{
    static uint32_t lastswitch = 0;
    static uint8_t seqno = 0;
    static uint16_t lastpattern = 0;  /* since variables are static, when switching patterns, the correct pattern will start on the second sequence */

    if (millis() < (lastswitch + speed))
    {
        /* not time to change pattern yet */
        return;
    }
    /* Save last switch timestamp */
    lastswitch = millis();

    for (uint8_t i = 0; i < 3; i++)
    {
        uint8_t state = (pattern >> (seqno * 3 + i)) & 1; /* get pattern bit */
        /* since value is multiple of 25, we can calculate time based on pattern position */
        /* i=0 is green, 1=blue, 2=red, green ON ticks is calculated as 50*(0+1)-1*25 = 25 ticks */
        uint8_t tick = 50 * (i + 1);
        if (state)
        {
            tick -=25;
        }
        PilotLamp(tick);
        Alarm_ResourceIsOn[i + 2] = state;
    }

    seqno++;
    seqno %= num_patterns;
}

/*******************************************************************************
* Function Name: PilotLamp
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void PilotLamp(uint8_t count)
{
    if (((tail + 1) % PL_BUF_SIZE) != head)
    {
        queue[tail] = count;
        tail++;
        tail = (tail % PL_BUF_SIZE);
    }
}

/*ISR code sensitive to changes, test thoroughly before flying */
ISR(PL_ISR)
{
    /* the interrupt service routine */
    static uint8_t state = 0;
    uint8_t h = head;
    uint8_t t = tail;

    if (state == 0)
    {
        if (h != t)
        {
            PL_PIN_ON;
            PL_CHANNEL += queue[h];
            h = ((h+1) % PL_BUF_SIZE);
            head = h;
            state=1;
        }
    }
    else if (state==1)
    {
        PL_PIN_OFF;
        PL_CHANNEL += PL_IDLE;
        state=0;
    }
}
#endif /* defined(PILOTLAMP)  */
